import pyb, sensor, image, time, os, struct, tracing, recognition

TRACING_THRESHOLD = (44, 95, -108, 127, -11, 127) # Grayscale threshold for dark things...
items = {"None":1, "bottle":2, "box":3, "fork":4, "gear":5, "toy":6}

uart = pyb.UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((240,240))
sensor.skip_frames(time=2000)
clock = time.clock()

while True:
    clock.tick()
    img = sensor.snapshot()

    label, value = recognition.recognize(img)
    value = int(value*100) + 1 # 0x00 is treated as end of message, avoid it

    img.scale(x_size=60, y_size=60)
    img = img.binary([TRACING_THRESHOLD])
    rho_err, theta_err, mag = tracing.find_line(img)

    serial_data =struct.pack("<bbbbb", items[label], value, rho_err, theta_err, mag)
    uart.write(b'\x81')
    uart.write(serial_data)
    uart.write(b'\x7f')

    print(label, value, rho_err, theta_err, mag)
